# coding=UTF-8

import numpy as np
import cv2
from  miscfunc import *
#from win32api import GetSystemMetrics

#def match_flann(desc1,desc2,kp1,kp2,img1,img2):
def match_flann(desc1,desc2):
    FLANN_INDEX_KDTREE = 0
    indexParams = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
    searchParams = dict(checks=50)   #
    flann = cv2.FlannBasedMatcher(indexParams, searchParams)
    matches = flann.knnMatch(desc1, desc2,k=2)
    # Need to draw only good matches, so create a mask
    matchesMask = [[0, 0] for i in range(len(matches))]
    # ratio test as per Lowe's paper
    for i, (m, n) in enumerate(matches):
        if m.distance < 0.7 * n.distance:
            matchesMask[i] = [1, 0]
    matches2=[matches[i][0] for i in range(len(matches)) if matchesMask[i][0] ]
    ##display
    """
    draw_params = dict(matchColor=(0, 255, 0),
                       singlePointColor=(255, 0, 0),
                       matchesMask=matchesMask,
                       flags=0)
    img=cv2.drawMatchesKnn(img1,kp1,img2,kp2,matches,None,**draw_params)
    sc_width=GetSystemMetrics(0)
    sc_height=GetSystemMetrics (1)
    height,width=img.shape[:2]
    if sc_width<width:
        factor=sc_width/width
        dist_height=factor*height
    if sc_height<dist_height:
        factor=sc_height/height
    factor=factor*0.8
    dist_height=int(factor*height)
    dist_width=int(factor*width)
    img=cv2.resize(img,(dist_width,dist_height))
    while (True):
        cv2.imshow("matches",img)
        if cv2.waitKey() & 0xff == ord("q"):
            break
    cv2.destroyAllWindows()
    """
    return matches2

"""
获取匹配点对的坐标
"""
def fetchCoordinate(kp1,kp2,matches):
    N=len(matches)
    #将匹配点对的坐标提取出来
    pt1=np.zeros((N,2))
    pt2=np.zeros((N,2))
    for i in range(N):
        pt2[i,:]=kp2[matches[i].trainIdx].pt
        pt1[i,:]=kp1[matches[i].queryIdx].pt
    return pt1,pt2


"""
计算相机2相对于相机1的方位
输入：
    1,pts1-->相机1的图像点（N*2）
    2,pts2-->相机2的对应匹配图像点（N*2）
    3,K-->相机的内参矩阵（3*3）
输出：
    1,R-->相机2相对于相机1的朝向，3*3旋转矩阵
    2,t-->相机2相对于相机1的位置，3*1列向量
    3,ratio-->Z>0的3D点的比率
"""
def estimateRelativePose(pts1,pts2,K,F):
    goodN=len(pts1)
    Kt=K.transpose()
    E=np.dot(Kt,np.dot(F,K))
    R1,R2,t=cv2.decomposeEssentialMat(E)
    Rs=[]; Rs.append(R1); Rs.append(R1); Rs.append(R2); Rs.append(R2)
    ts=[]; ts.append(t); ts.append(-t); ts.append(t); ts.append(-t)
    # 对于四个可能解，选出实际可行的那个。即能将重建3D点放置在两相机的前方
    P1=np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0]]);    P1=np.dot(K,P1)
    numNegtives=np.zeros(4)
    for i in range(4):
        P2=np.hstack([Rs[i],ts[i]]);    P2=np.dot(K,P2)
        #计算重建3D点(世界坐标系中)
        homoM1=cv2.triangulatePoints(P1,P2,pts1[None,:,:],pts2[None,:,:])
        M1=cv2.convertPointsFromHomogeneous(homoM1.transpose())
        M1=M1.reshape([goodN, 3])
        #将3D点变换到相机2的坐标系中
        M2=np.dot(M1,Rs[i].transpose())+ts[i].transpose()
        # 计算其中Z<0的3D点的数量
        numNegtives[i]=max(np.sum(M1[:,2]<0),np.sum(M2[:,2]<0))
    ##选择其中Z<0点最少的那组[R,t]
    minIdx=np.argmin(numNegtives)
    ratio=1-numNegtives[minIdx]/goodN
    return Rs[minIdx],ts[minIdx],ratio


